function [ u_h condK ] = codinaSolveMatrix_BC_Lagrange( boundarySamplePoints, boundaryConditions,  boundaryShapFuncVal, ...
                                                boundaryNears, bc_Type, xNodes, s_nears, p_s, dp_s, xSamples, w_s )
%CODINASOLVEMATRIX_BC_Lagrange
%   boundaryShapFuncVal: The Shape functions value for each boundary sample
%                        point.
%   boundaryConditions : The Dirichlet  voundary condition values
%                        (over the sample points)
%   boundaryNears      : The nearest nodes for each boundary sample point.
%   boundarySamples    : The Dirichlet boundary samples
%   bc_Type  : The type of boundary condition to be used, 0 is homogeneus,
%              1 is standard Codina's problem
%   xNodes   : Node points.
%   xSamples : Sample points.
%   id_bd    : indices for the boundary nodes with dirichlet BC's.
%   w_s      : Gaussian integration weights for each sample point.
%   s_nears  : For each sample point, the nearest nodes.
%   p_s      : Shape function of each node evaluated at each sample point.
%   dp_s     : Shape function Gradient of each node evaluated at each
%              sample point.


% Number of node points
totalNodes      = size(xNodes,1);       
% Number of sample points
totalSamples    = size(xSamples,1);     
% Number of boundary sample points
numberBoundarySamplePoints = size (boundarySamplePoints,1);


%% Stiffness matrix assembly-----------------------------------------------

% The stiffness matrix is enlarged with 1 aditional entry for each Lagrange
% multiplier associated with an imposed boundary condition
K = zeros(totalNodes + numberBoundarySamplePoints, ...
    totalNodes + numberBoundarySamplePoints);

% For each sample point (Gauss Point)
for k = 1 : totalSamples;
    % Vector with the nearest node points of the k-ith gauss point.
    k_near = s_nears{k};
    % Gradient of the Shape function at the k-ith gauss point.
    dp_k   = dp_s{k};
    % Sum the contribution of the (\nabla u \cdot \nabla v)
    K(k_near,k_near) = K(k_near,k_near) + (dp_k*dp_k') * w_s(k);
end

for k = 1 : numberBoundarySamplePoints;
    % Search index for index of the current Lagrange multiplier to impose
    indexLagrange = totalNodes + k;
    % Vector with the nearest node points of the k-ith boundary sample
    % point.
    k_near = boundaryNears{k};    
    % Shape function values of the near nodes corresponding to the 
    % k boundary sample point.
    p_k = boundaryShapFuncVal{k};
    
    % Sum the contribution of the  \delta(indexLagrange) u(a)
    for a = 1 : numel(k_near);
        % Acumulates the integral value
        K(k_near(a),indexLagrange) = K(k_near(a),indexLagrange) +...
            p_k(a);
        K(indexLagrange,k_near(a)) = K(k_near(a),indexLagrange);
    end;    
end;


%% Right hand side assembly------------------------------------------------
% Right hand side vector
rhs  = zeros(totalNodes + numberBoundarySamplePoints, 1);

%% Homogeneus boundary condition problem
if (bc_Type == 0);
    % for each sample point k
    for k = 1 : totalSamples;
        % Gets the neares Nodes of the k-ith sample point.
        k_near = s_nears{k};
        % number of neares nodes of the k-ith sample point.
        n_k    = length(k_near);
        % Shape function value at the k-ith sample point.
        p_k    = p_s{k};
        % Gaussian integration weight for the k-ith sample point.
        w_gk   = w_s(k);
        % The external "force" (-2y)
        val    = -2.0 * xSamples(k,2) * w_gk;
        
        % loop on the neighbors of the i-th gauss points
        for ia = 1 : n_k;
            i      = k_near(ia);
            rhs(i) = rhs(i) + p_k(ia) * val;
        end;
    end;
end;


%% Load vector for the Lagrange multiplier - rhs Lagrange

for k = 1 : numberBoundarySamplePoints;
    % Search index for index of the current Lagrange multiplier to impose
    indexLagrange = totalNodes + k;
    rhs (indexLagrange) = boundaryConditions(k);
end

%% Solve the linear system of equation.
condK = cond(K);
u_h   = K \ rhs;

end

